import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.robotics.RangeFinder;
import com.ridgesoft.robotics.sensors.ParallaxPing;

public class Ultrasound implements Runnable{
	private static int LEFT_ULTRASOUND_PORT = 4;
	private static int RIGHT_ULTRASOUND_PORT = 5;
	private static int CENTER_ULTRASOUND_PORT = 6;
	
	private int _mCenterData;
	private int _mLeftData;
	private int _mRightData;
	
	private RangeFinder _mLeftUS;
	private RangeFinder _mRightUS;
	private RangeFinder _mCenterUS;
	
	private static final Ultrasound instance = new Ultrasound();
	
	public Ultrasound()
	{		
	}
	public void init()
	{
		_mLeftUS = new ParallaxPing(IntelliBrain.getDigitalIO(LEFT_ULTRASOUND_PORT));
	    _mRightUS = new ParallaxPing(IntelliBrain.getDigitalIO(RIGHT_ULTRASOUND_PORT));
		_mCenterUS = new ParallaxPing(IntelliBrain.getDigitalIO(CENTER_ULTRASOUND_PORT));
		//NOTE: May have to put ThreadManager.registerThread(new UpdateUS(), 8); 
		//but then we would need to make an internal updateUS class
	}
	public int getRight()
	{
		return _mRightData;
	}
	public int getLeft()
	{
		return _mLeftData;
	}
	public int getCenter()
	{
		return _mCenterData;
	}
	public void UpdateSensors()
	{
		_mLeftUS.ping();
		_mRightUS.ping();
		_mCenterUS.ping();
			
		try
		{
			Thread.sleep(100);
		}
		catch(InterruptedException e)
		{
			e.printStackTrace();
		}
		_mRightData = (int)_mRightUS.getDistanceInches();
		_mLeftData = (int)_mLeftUS.getDistanceInches();
		_mCenterData = (int)_mCenterUS.getDistanceInches();
			
	}
	@Override
	public void run() 
	{
		init();
		Thread.yield();		
		while(true)
		{
			UpdateSensors();	
		}
	}
	  public static Ultrasound getInstance() {
	        return instance;
	    }

	
}
